/*
tello单机测试代码，测试其位置跟踪以及避障功能
*/
/*
	tello石景山飞行试验速度控制算法
	算法采用P控制进行定点跟踪，采用人工势场法进行实时避障与机间避碰
*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseArray.h"
// #include "tello_control/Position.h"
#include <math.h>
#include <iostream>
#define PI 3.1415926
using namespace std;

double t = 0, tstart = 0;
int i, j, flag = 0;
double K = -1.3;
double u[50][3];
double tello_pos[50][3];										   // 定义tello无人机的位置，最多50架
double tello1_yaw, tello2_yaw, tello3_yaw, tello4_yaw, tello5_yaw; // 四机航向角
double yaw1, yaw2, yaw3, yaw4, yaw5;
double expectAngle1 = 0, expectAngle2 = 0, expectAngle3 = 0, expectAngle4 = 0, expectAngle5 = 0;
double p[3] = {0};																			  // 位置跟踪项,位置避障偏移量
double h[50][3] = {0};																	  // 编队偏移项
double v[50][3] = {0};																	  // 速度补偿项
double F[50][3] = {0}, Fp[3] = {0};														  // 避碰补偿项
double D[50][50] = {0}, Fx[50][50] = {0}, Fy[50][50] = {0}, Fpx[50] = {0}, Fpy[50] = {0}; // 机间距离,斥力函数等
geometry_msgs::PoseArray obstacle;									// 用于储存障碍物位置的坐标,半径
int n1 = 1, n2 = 0;											// n1代表编队飞机的个数，n2代表障碍物的个数
double dp_ob[50] = {0},ap_ob[50] = {0};																	  // 用于储存飞行中心与障碍物之间的距离

double K_rep = 0.4; // 斥力场系数
double ro = 0.6;	// 作用范围

double r1 = 1, r2 = 1, r3 = 0.6, w = 0.4; // 定义偏移半径与角速度
double vmax = 1.0;							 // 最大速度
double t1 = 109, t2 = 181, t3 = 80;			 // t1,t2代表旋转时间的两个时间点，t3代表节点丢失的时间点

tello_control::Position center_pos; // 编队中心从外部算法接收

geometry_msgs::Twist cmd_vel[50];

int command = 0;

// 订阅tellogc的无人机位置信息
void cb_readPos_0(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	double Roll, Pitch, Yaw;
	geometry_msgs::PoseStamped pos;
	pos = *msg;	
	tello_pos[0][0] = pos.pose.position.x;
	tello_pos[0][1] = pos.pose.position.y;
	tello_pos[0][2] = pos.pose.position.z;

	tf::Quaternion quat; // 将四元数转换为偏航角等信息
	tf::quaternionMsgToTF(pos.pose.orientation, quat);
	tf::Matrix3x3(quat).getRPY(Roll, Pitch, Yaw);
	Yaw = Yaw * 180 / 3.14159;
	Yaw = atan2(sin(Yaw), cos(Yaw));
	tello1_yaw = Yaw;
}

void cb_command(const std_msgs::Int32::ConstPtr &msg) // 接收指令信息
{
	std_msgs::Int32 cmd_data;
	cmd_data = *msg;
	command = cmd_data.data;
	cout << "receive command: " << command << endl;
	if (command == 3 && flag == 0)
	{
		tstart = ros::Time::now().toSec();
		flag = 1;
	}
}

void vel_limit(double u[], double n) // 速度限制函数
{
	int i;
	for (i = 0; i < 2; i++)
	{
		if (u[i] > n)
			u[i] = n;
		if (u[i] < -n)
			u[i] = -n;
	}
}

void vel_control1() // 计算控制输入，中心点编队跟踪
{
	// 计算速度控制指令
	u[0][0] = K * (tello_pos[0][0] - p[0]);
	u[0][1] = K * (tello_pos[0][1] - p[1]);
}

geometry_msgs::Pose pose1;
int main(int argc, char **argv)
{
	ros::init(argc, argv, "obstacle_test");
	ros::NodeHandle nh("");

	ros::Subscriber cmd_sub = nh.subscribe("/command", 10, cb_command); // 订阅控制命令信息

	// 从地面站订阅tello的当前位置（mocap - gc - tello）
	ros::Subscriber pos_sub0 = nh.subscribe("/tello5/pose", 10, cb_readPos_0);
	// 发布当前的中心点位置信息(指令中心位置)
	// ros::Publisher center_pub = nh.advertise<tello_control::Position>("/tello_center", 10);

	ros::Publisher vel_pub0 = nh.advertise<geometry_msgs::Twist>("/tello5/cmd_vel", 10);

	ros::Publisher takeoff_pub0 = nh.advertise<std_msgs::Empty>("/tello5/takeoff", 10);

	ros::Publisher land_pub0 = nh.advertise<std_msgs::Empty>("/tello5/land", 10);

	ros::Publisher obstacle_pub = nh.advertise<geometry_msgs::PoseArray>("/obstacle", 10);

	// 发布当前tello位置，记录数据用

	ros::Rate loop_rate(100);

	while (ros::ok())
	{
		double t = ros::Time::now().toSec() - tstart;
		obstacle.poses.clear();
		if (command == 0) //Fx[i][j]读取初始偏航角并储存
		{
			std_msgs::Empty takeoff_cmd;
			takeoff_pub0.publish(takeoff_cmd);
		}
		else// 指令速度给0
		{
			p[0] = 1.5;
			p[1] = 4;
			p[2] = 0.2;
			pose1.position.x = p[0];
			pose1.position.y = p[1];
			pose1.position.z = p[2];
			obstacle.poses.push_back(pose1);
			vel_control1();
            cmd_vel[0].linear.x = u[0][0];
			cmd_vel[0].linear.y = u[0][1];
            vel_pub0.publish(cmd_vel[0]);
			obstacle_pub.publish(obstacle);
		}
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}